/*
 * @file px4_simple_flight.cpp
 * @brief PX4无人机起飞-悬停-降落控制(C++/MAVROS)
 * 功能：自主解锁、切Offboard模式、起飞至指定高度、悬停固定时间、自动降落
 */
#include "flight_control_config.h"

// 飞行状态初始化
FlightPhase phase = WAITING;
// 人为控制飞行状态
FlightPhase int_to_enum(const int request){
    if(request==1) return INIT; // 请求解锁

    // 若飞机未完成解锁却试图请求起飞或悬停或使用虚拟摇杆，则回到waiting
    int phase_type = phase;
    if(phase_type < 3 && (request == 3 || request == 4 || request == 8)) return WAITING;

    if(request==3) return TAKEOFF; // 请求起飞
    if(request==4) {   // 请求悬停
        target_pose.type_mask = ControlMasks::VELOCITY_CONTROL;
        return HOVER;
    }
    if(request==5) {   // 请求降落
        target_pose.type_mask = ControlMasks::VELOCITY_CONTROL;
        return LAND; 
    }
    if(request==8) {  // 请求使用虚拟摇杆
        last_target_request = ros::Time::now();
        return VSTICK;
    }

    return WAITING;
}

// 飞行状态类型转换
string enum_to_str(const FlightPhase phase){
    if(phase == WAITING) return "WAITING";
    if(phase == INIT) return "INITING";
    if(phase == ARMING) return "ARMING";
    if(phase == ARM_FIN) return "ARM_FIN";
    if(phase == TAKEOFF) return "TAKEOFF";
    if(phase == HOVER) return "HOVERING";
    if(phase == LAND) return "LANDING";
    if(phase == DISARM) return "DISARM";
    if(phase == VSTICK) return "VSTICKING";
    return "UNKNOW";
}


// 全局变量存储无人机状态
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg) {
    current_state = *msg;
}

// 全局变量存储无人机位置
// geometry_msgs::PoseStamped current_local_pose;
nav_msgs::Odometry current_local_statu;
geometry_msgs::Point current_local_pose;
void get_local_pose(const nav_msgs::Odometry::ConstPtr& msg) {
    current_local_statu = *msg;
    current_local_pose = current_local_statu.pose.pose.position;
}

// 控制无人机飞行
bool flight_cb(px4_flight::flight_status_control::Request &request, 
        px4_flight::flight_status_control::Response &response) {
    int statu = request.statu;
    phase = int_to_enum(statu);
    response.success = true;
    return true;
}

// 目标速度or位置指令
mavros_msgs::PositionTarget target_pose;
// 记录最后一次接收到指令的时间，若超时则返回HOVER模式
ros::Time last_target_request;
bool target_cb(px4_flight::flight_target_control::Request &request, 
        px4_flight::flight_target_control::Response &response){
    last_target_request = ros::Time::now();
    if(request.mode == "position1"){ // 位置控制 本地坐标系
        target_pose.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
        target_pose.type_mask = ControlMasks::POSITION_WITH_YAW_CONTROL;
        target_pose.position.x = request.x_vp + current_local_pose.x;
        target_pose.position.y = request.y_vp + current_local_pose.y;
        target_pose.position.z = request.z_vp + current_local_pose.z;
        target_pose.yaw = request.yaw;
    }   // 位置控制疑似不支持使用机体坐标系
    else if(request.mode == "position2"){ // 位置控制 本地坐标系
        target_pose.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
        target_pose.type_mask = ControlMasks::POSITION_WITH_YAW_CONTROL;
        target_pose.position.x = request.x_vp;
        target_pose.position.y = request.y_vp;
        target_pose.position.z = request.z_vp;
        target_pose.yaw = request.yaw;
    }
    else if(request.mode == "velocity"){ // 速度控制
        target_pose.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED;
        target_pose.type_mask = ControlMasks::VELOCITY_WITH_YAW_RATE_CONTROL;
        target_pose.velocity.x = request.x_vp;
        target_pose.velocity.y = request.y_vp;
        target_pose.velocity.z = request.z_vp;
        target_pose.yaw_rate = request.yaw;
    }   // yaw和yaw_rate，角度和角速度最好使用一种
    response.success = true;
    return true;
}


void px4_flight_control() {
    // ros句柄
    ros::NodeHandle nh;

    while(!ros::ok()){

    }

    // 创建ROS对象
    ros::Publisher show_status_pub = nh.advertise<px4_flight::show_status>(
        "show_status", 10);
    ros::ServiceServer flight_statu_srv = nh.advertiseService(
        "flight_status", flight_cb);
    ros::ServiceServer flight_target_srv = nh.advertiseService(
        "flight_target", target_cb);
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>(
        "mavros/state", 10, state_cb);
    ros::Subscriber local_pos_sub = nh.subscribe<nav_msgs::Odometry>(
        "/mavros/global_position/local", 10, get_local_pose);
    ros::Publisher local_pos_pub = nh.advertise<mavros_msgs::PositionTarget>(
        "/mavros/setpoint_raw/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>(
        "mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>(
        "mavros/set_mode");
    ros::ServiceClient set_land_client = nh.serviceClient<mavros_msgs::CommandTOL>(
        "/mavros/cmd/land");

    //初始化目标位置
    // 设置target_pose坐标系（LOCAL_NED 或 BODY_NED）
    target_pose.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED;

    // 仅启用速度控制
    target_pose.type_mask = ControlMasks::VELOCITY_CONTROL;
        
    // target_pose.header.stamp = ros::Time::now();
    // target_pose.header.frame_id = "odom";  // 或"map"

    target_pose.velocity.x = 0;
    target_pose.velocity.y = 0;
    target_pose.velocity.z = 0;
    // target_pose.yaw = 0.0;
    // target_pose.yaw_rate = 0.0;

    // 控制频率(必须>2Hz)
    ros::Rate rate(50.0);

    // 等待MAVROS与飞控连接
    while(ros::ok() && !current_state.connected) {
        ros::spinOnce();
        rate.sleep();
    }
    ROS_INFO("FCU connected");

    // 发送预设点激活Offboard
    for(int i=100; ros::ok() && current_state.connected && i>0; --i){
        local_pos_pub.publish(target_pose);
        ros::spinOnce();
        rate.sleep();
    }

    // 设置Offboard模式
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    
    // 解锁
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    // 着陆
    mavros_msgs::CommandTOL land_cmd;
    land_cmd.request.altitude = 0;

    // 状态变量初始化
    ros::Time last_request = ros::Time::now();
    ros::Time hover_start_time;

    // 飞行状态播报
    px4_flight::show_status show_status;

    double theta = 0;
    double omega = 2 * M_PI / 5;  // 角速度（弧度/秒）
    double r = 3;    //半径
    
    // 主控制循环
    while(ros::ok() && current_state.connected) {
        // 持续发布无人机状态
        show_status.statu = enum_to_str(phase);
        show_status_pub.publish(show_status);
        // 持续发布目标位置
        local_pos_pub.publish(target_pose);
        float dist = abs(current_local_pose.z - hover_alt);
        // ROS_INFO("Flying");
        // 状态机逻辑
        switch(phase) {
        case WAITING:
            break;
        
        case INIT:
            if(ros::Time::now() - last_request > ros::Duration(5.0)) {
                if(set_mode_client.call(offb_set_mode) && 
                   offb_set_mode.response.mode_sent) {
                    ROS_INFO("Offboard enabled");
                    phase = ARMING;
                }
                last_request = ros::Time::now();
            }
            break;
            
        case ARMING:
            if(!current_state.armed && 
               (ros::Time::now() - last_request > ros::Duration(5.0))) {
                if(arming_client.call(arm_cmd) && 
                   arm_cmd.response.success) {
                    ROS_INFO("Vehicle armed");
                    phase = TAKEOFF;
                    // phase = ARM_FIN;
                    last_request = ros::Time::now();
                }
            }
            break;
            
        case TAKEOFF:
            // 高度到达阈值判断
            if(current_state.armed && 
               current_state.mode == "OFFBOARD") {
                if(ros::Time::now() - last_request > ros::Duration(takeoff_timeout)) {
                    ROS_ERROR("Takeoff timeout!");
                    phase = LAND;
                } 
                else if(hover_alt - current_local_pose.z > 0.1) {  // 此处替换为地面接触检查
                    target_pose.velocity.z = (hover_alt - current_local_pose.z) / 2;
                }
                // 实际项目中应添加位置反馈检查
                else if(current_local_pose.z + 0.1 >= hover_alt) {  // 此处替换为高度检查逻辑
                    target_pose.velocity.z = 0;
                    hover_start_time = ros::Time::now();
                    phase = HOVER;
                    ROS_INFO("Reached target height, Hovering...");
                }
            }
            break;
            
        case HOVER:
            target_pose.velocity.x = 0;
            target_pose.velocity.y = 0;
            target_pose.velocity.z = 0;
            break;
        
        case VSTICK:
            if(ros::Time::now() - last_target_request > ros::Duration(vistik_timeout)){
                // 若超时，则将模式切换为HOVERING
                ROS_INFO("VISTIK timeout! Change mode to HOVERING");
                target_pose.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED;
                target_pose.type_mask = ControlMasks::VELOCITY_CONTROL;
                phase = HOVER;
            }
            break;
            
        case LAND:
            // 高度<0.1m认为降落完成
            // 实际项目应添加位置反馈检查
            // else if(current_local_pose.pose.position.z < 0.3 && current_local_pose.pose.position.z >= 0.1) {  // 此处替换为地面接触检查
            //     // target_pose.type_mask = ControlMasks::POSITION_CONTROL;
            //     // target_pose.position.x = 0;
            //     // target_pose.position.y = 0;
            //     // target_pose.position.z = 0;
            //     target_pose.velocity.z = -0.05;
            // }
            if(current_local_pose.z > 0.05) {  // 此处替换为地面接触检查
                target_pose.velocity.z = -current_local_pose.z / 4;
            }
            else if(current_local_pose.z <= 0.05) {  // 此处替换为地面接触检查
                // 发送降落指令
                set_land_client.call(land_cmd);
                if(land_cmd.response.success) {
                    ROS_INFO("Landing enabled");
                    phase = DISARM;
                    ros::Duration(5.0).sleep();
                }
            }
            break;
            
        case DISARM:
            arm_cmd.request.value = false;  // 发送上锁命令
            if(arming_client.call(arm_cmd) && arm_cmd.response.success) {
                ROS_INFO("Vehicle disarmed");
                // return 0;  // 程序正常结束
                phase = WAITING;
            }
            break;
        }
        
        ros::spinOnce();
        rate.sleep();
    }
}


// int main(int argc, char **argv) {
//     if(argc == 2){
//         hover_alt = atof(argv[1]);
//         printf("Change the hover_alt to:%f\n", hover_alt);
//     }
//     // ROS节点初始化
//     ros::init(argc, argv, "px4_flight_control");
    
//     px4_flight_control();

//     return 0;
// }